
/* Includes ------------------------------------------------------------------*/
#include "GY33ColorSensor.h"

GY33ColorSensor::GY33ColorSensor(void)
{
  m_Device_Address = COLORSENSOR_DEFAULT_ADDRESS;
}

GY33ColorSensor::GY33ColorSensor(uint8_t address)
{
  m_Device_Address = address;
}
void GY33ColorSensor::SensorInit(void)
{
   delay(20);
   Wire.begin();
   writeReg(MODULE_CONFIG, 0x50);
}
void GY33ColorSensor::update(void)
  {
    uint8_t ColorData[16] = {0};
    readData(RAW_RED_H, ColorData, sizeof(ColorData));
    m_OrigRedvalue   = (uint16_t)ColorData[0]<<8|ColorData[1];
    m_OrigGreenvalue = (uint16_t)ColorData[2]<<8|ColorData[3];
    m_OrigBluevalue  = (uint16_t)ColorData[4]<<8|ColorData[5];
    m_OrigClearvalue = (uint16_t)ColorData[6]<<8|ColorData[7];
    m_Red = ColorData[COLOR_R];
    m_Green = ColorData[COLOR_G];
    m_Blue = ColorData[COLOR_B];
  }
 uint8_t GY33ColorSensor::ColorIdentify(void)
 {
    uint8_t result,r,g,b;
 /*****************小数据远距离判断策略****************/
    if(m_OrigRedvalue<1200 && m_OrigGreenvalue<1700 && m_OrigBluevalue<1200)
    {
       if(m_OrigBluevalue<m_OrigRedvalue && m_OrigRedvalue<=m_OrigGreenvalue)// b<r<g   g最大,b最小，green 和 yellow black,white区分
       {
           if((m_OrigGreenvalue + m_OrigRedvalue + m_OrigBluevalue >300) && (m_OrigGreenvalue > 1.5*m_OrigRedvalue) && (m_OrigGreenvalue > m_OrigBluevalue + m_OrigBluevalue))
           {
             result = GREEN;//判定绿色
           }
           else if((m_OrigGreenvalue > m_OrigBluevalue + m_OrigBluevalue) && (m_OrigRedvalue > m_OrigBluevalue + m_OrigBluevalue) && (m_OrigRedvalue +  m_OrigGreenvalue + m_OrigBluevalue>120))//芯片对蓝色不太灵敏，所以导致对黄色特别好识别 r>2b g>2b
           {
             result = YELLOW;//判定黄色
           }
           else if(m_OrigRedvalue>450 && m_OrigGreenvalue>580 && m_OrigBluevalue>320)//调小数据提高白色识别高度
           {
             result = WHITE;//
           }
           else
           {
             result = BLACK;//30mm以外认为是黑色
           }
       }
       else if(m_OrigGreenvalue<m_OrigRedvalue && m_OrigBluevalue<m_OrigGreenvalue)//r>g>b  r最大,b最小,red 和 orange black区分
       {
         if(m_OrigRedvalue + m_OrigGreenvalue + m_OrigBluevalue > 220)
         {
            // if(m_OrigRedvalue > 2.4*m_OrigGreenvalue && m_OrigGreenvalue > 1.4*m_OrigBluevalue)//为了满足远距离要求，判定条件由 r>3g g>2b改为r>2.6*g g>1.4*b.
            if(m_OrigGreenvalue < 2.4*m_OrigBluevalue)//用b数据判断效果更加好  将参数2.4改小可以提高橙色识别高度，降低红色识别高度
            {
              result = RED;//red 
            }
            //else if(m_OrigRedvalue > 1.3*m_OrigGreenvalue && m_OrigGreenvalue > 2.3*m_OrigBluevalue)//为了满足远距离要求，判定条件由 r>2g g>3b改为r>1.3*g g>2.3*b.
            else  if(m_OrigGreenvalue >= 2.4*m_OrigBluevalue)//用b数据判断效果更加好
            {
               result = ORANGE;//orange r>1.8*g g>3*b
            }
            else
            {
              result = BLACK/*PINKE*/;
            }
         }
         else
         {
           result = BLACK;//否则距离太远判定黑色
         }
       }
      else if(m_OrigGreenvalue <= m_OrigBluevalue && m_OrigRedvalue<m_OrigBluevalue)//b>g b>r//包括b>g>r b>r>g//蓝色，紫色区分
      {
          result = BLUE;//判定蓝色 删除紫色，都判断为蓝色
        // if(m_OrigBluevalue>=m_OrigRedvalue+m_OrigRedvalue)//b>=2*r
        // {
        //   result = BLUE;//判定蓝色
        // }
        // else
        // {
        //   result = BLUE/*PURPLE*/;//判定紫色//删除紫色
        // }
      }
       else if(m_OrigRedvalue<=m_OrigGreenvalue && m_OrigBluevalue>=m_OrigRedvalue)// r<b<g//g max b middle r min 蓝色，绿色，紫色，黑色
       {
         if(m_OrigRedvalue + m_OrigGreenvalue + m_OrigBluevalue>400)
         {
           if(m_OrigGreenvalue>2.2*m_OrigRedvalue && m_OrigGreenvalue>2.2*m_OrigBluevalue)
           {
              result = GREEN;
           }
           else
           {
             result = BLUE;
           }
         }
         else
         {
           result = BLACK;//30mm以外认为是黑色
         }
       }
       else if((m_OrigGreenvalue < m_OrigRedvalue + m_OrigBluevalue) && (m_OrigGreenvalue + m_OrigRedvalue + m_OrigBluevalue >700))//
       {
           result = WHITE;//判定白色
       }
       else if(m_OrigGreenvalue + m_OrigRedvalue + m_OrigBluevalue <1000)
       {
           result = BLACK;//判定黑色
       }
      else
      {
        result = BLACK;
      }
    }
    /************** 稍大数据中距离判断策略***************/
    else if(m_OrigBluevalue>m_OrigGreenvalue && m_OrigBluevalue>m_OrigRedvalue)// b>g  b>r//包括b>g>r b>r>g//蓝色，紫色区分
    {
        result = BLUE;//在这一范围内都认为是蓝色
      // if(m_OrigBluevalue < m_OrigRedvalue+m_OrigRedvalue)//紫色成分中红色所占比例比较大
      //  {
      //    result = BLUE/*PURPLE*/;//删除紫色  只要满足b>g>r都认为是蓝色
      //  }
      //  else                          //蓝色成分中红色所占比例比较小
      //  {
      //    result = BLUE;
      //  }
    }
    else if(m_OrigRedvalue > m_OrigGreenvalue && m_OrigGreenvalue > m_OrigBluevalue)// r>g>b r<2000
    {
        if(m_OrigGreenvalue < 2.5*m_OrigBluevalue)//用b数据判断效果更加好
       {
            result = RED;//red 
       }
       else if(m_OrigGreenvalue >= 2.5*m_OrigBluevalue)//用b数据判断效果更加好
       {
            result = ORANGE;//orange r>1.8*g g>3*b
        }
    }
   /************** 超大数据近距离判断策略***************/
    else
    {
      r = m_OrigRedvalue/m_OrigClearvalue;
      g = m_OrigGreenvalue/m_OrigClearvalue;
      b = m_OrigBluevalue/m_OrigClearvalue;

      if(r>10&&g<=4&&b<=1||r>10&&g<=3&&b<=1)
      {
        result = RED;
      }
      else if(r>10&&g<=4&&b<=4)
      {
        result = RED/*PINKE*/;//删除粉红色
      }
      else if(r<3&&g<=4&&b<3)
      {
          result = BLACK;
      }
      else if(r<=5&&g>10&&b<5)
      {
        result = GREEN;
      }
      else if((r<5&&g<5&&b>10)||r<=3&&g>=8&&b>=8)
      {
        result = BLUE;
      }
      else if((r>=5&&g>6&&b>=7))
      {
        result = BLUE/*PURPLE*/;//删除紫色
      }
      else if((r<=4&&g>=10&&b>=6))
      {
        result = BLUE/*CYAN*/;//删除青色
      }
      else if(r<=8&&r>=6&&g>=6&&b<2)
      {
        result = YELLOW;
      }
      else if(r<=8&&r<=8&&b<2)
      {
        result = /*GOLD*/ORANGE;//删除金色
      }
      else if(r>=10&&g<=7&&b<2)
      {
        result = ORANGE;
      }
      else if(r>=4&&g>=9&&b>=4)
      {
        result = WHITE;
      }
      else
      {
        result = WHITE;
      }
    }
  /***************move filter***************/
    // Temp_[cnt++] = result;
    // if(cnt>=3) cnt = 0;
    // result = (Temp_[0] + Temp_[1] + Temp_[2])/3;
    return result;
 }

int8_t GY33ColorSensor::writeReg(int16_t reg, uint8_t data)
{
  int8_t return_value = 0;
  return_value = writeData(reg, &data, 1);
  return(return_value);
}

int8_t GY33ColorSensor::readData(uint8_t start, uint8_t *buffer, uint8_t size)
{
  int16_t i = 0;
  int8_t return_value = 0;
  Wire.beginTransmission(m_Device_Address);
  return_value = Wire.write(start);

  if(return_value != 1)
  {
    return(I2C_ERROR);
  }
  return_value = Wire.endTransmission(false);

  if(return_value != 0)
  {
    return(return_value);
  }
  delayMicroseconds(1);
  /* Third parameter is true: relase I2C-bus after data is read. */
  Wire.requestFrom(m_Device_Address, size, (uint8_t)true);
  while(Wire.available() && i < size)
  {
    buffer[i++] = Wire.read();
  }
  delayMicroseconds(1);
  if(i != size)
  {
    return(I2C_ERROR);
  }
  return(0); //return: no error 
}

int8_t GY33ColorSensor::writeData(uint8_t start, const uint8_t *pData, uint8_t size)
{
  int8_t return_value = 0;
  Wire.beginTransmission(m_Device_Address);
  return_value = Wire.write(start); 
  if(return_value != 1)
  {
    return(I2C_ERROR);
  }
  Wire.write(pData, size);  
  return_value = Wire.endTransmission(true); 
  return(return_value); //return: no error                     
}
void GY33ColorSensor::doWhiteBalance()
{
}
int8_t GY33ColorSensor::getColor()
{
  return ColorIdentify();
} 



